/******************************************************************************
*  Nano-RK, a real-time operating system for sensor networks.
*  Copyright (C) 2007, Real-Time and Multimedia Lab, Carnegie Mellon University
*  All rights reserved.
*
*  This is the Open Source Version of Nano-RK included as part of a Dual
*  Licensing Model. If you are unsure which license to use please refer to:
*  http://www.nanork.org/nano-RK/wiki/Licensing
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation, version 2.0 of the License.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU General Public License for more details.
*
*  You should have received a copy of the GNU General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*
*  Contributing Authors (specific to this file):
*  Zane Starr
*******************************************************************************/


#include <nrk.h>
#include <include.h>
#include <ulib.h>
#include <stdio.h>
#include <avr/sleep.h>
#include <hal.h>
#include <nrk_error.h>
#include <nrk_events.h>
#include <nrk_timer.h>

#define SPIN_WAIT_TIME 980//960 //908

inline void spin_wait_s(uint16_t wait_sec) {
  int16_t s_cnt, ms_cnt;
    for (s_cnt = 0; s_cnt < wait_sec; s_cnt++) {
        for (ms_cnt = 0; ms_cnt < SPIN_WAIT_TIME; ms_cnt++) {
            //spinwait 1ms
            nrk_spin_wait_us(1000);
        }
    }
}
/*
inline void print_time(void) {
  nrk_time_t c_time;
  printf("Current Time: %u\r\n", c_time.secs);
}
*/

NRK_STK Stack1[NRK_APP_STACKSIZE];
nrk_task_type TaskOne;
void Task1(void);

NRK_STK Stack2[NRK_APP_STACKSIZE];
nrk_task_type TaskTwo;
void Task2 (void);

NRK_STK Stack3[NRK_APP_STACKSIZE];
nrk_task_type TaskThree;
void Task3 (void);

void nrk_create_taskset();

nrk_sem_t *sema1;
nrk_sem_t *sema2;

int
main ()
{
  nrk_setup_ports();
  nrk_setup_uart(UART_BAUDRATE_115K2);

  printf( "Starting up...\r\n" );

  nrk_init();
  // Need to make semaphores... before we can assign tasks to them!
  sema1 = nrk_sem_create(1,0);
  sema2 = nrk_sem_create(1,0);
  printf( "Starting up1...\r\n" );
  nrk_led_clr(ORANGE_LED);
  nrk_led_clr(BLUE_LED);
  nrk_led_set(GREEN_LED);
  nrk_led_clr(RED_LED);
   printf( "Starting up2...\r\n" );
  nrk_time_set(0,0);
  printf( "Starting up3...\r\n" );
  nrk_create_taskset ();
  printf( "Starting up4...\r\n" );
  //make sure initial ceilings are set to zero!
  nrk_start();
  
  return 0;
}


void Task1()
{
uint16_t cnt;
int8_t v;

//printf( "My node's address is %d\r\n",NODE_ADDR );

//  cnt=0;
  while(1) {
//  nrk_sem_pend(sema2);
	v = nrk_sem_pend(sema1);
    // Critical Section
	
    if(v==NRK_ERROR) nrk_kprintf( PSTR("T1 error pend\r\n"));
    spin_wait_s(5);
	// wait some time inside semaphore to show the effect

    // End Critical Section
	v = nrk_sem_post(sema1);	
//   v = nrk_sem_post(sema2);
//  spin_wait_s(1);
	if(v==NRK_ERROR) nrk_kprintf( PSTR("T1 error post\r\n"));
	nrk_wait_until_next_period();
	}
}

void Task2()
{
  uint8_t cnt;
  int8_t v;

  while(1) {
//        nrk_kprintf( PSTR("Task2 attempting access to semaphore\r\n"));
    v = nrk_sem_pend(sema2);    
    spin_wait_s(5);
//    v = nrk_sem_pend(sema2);
        //Begin Critical Section

	    if(v==NRK_ERROR) nrk_kprintf( PSTR("T2 error pend\r\n"));
//        nrk_kprintf( PSTR("Task2 holding semaphore\r\n"));
       	// wait some time inside semaphore to show the effect
//    	spin_wait_s(2);	

        // End of Critical Section
    	v = nrk_sem_post(sema2);       
//      nrk_sem_post(sema1);
//      spin_wait_s(1);

        // Give the scheduler a chance to breathe before this check
    	if(v==NRK_ERROR) nrk_kprintf( PSTR("T2 error post\r\n"));
//        nrk_kprintf( PSTR("Task2 released semaphore\r\n"));
        nrk_wait_until_next_period();
   }
}

void Task3()
{
  uint8_t cnt;
  int8_t v;

  while(1) {
        v = nrk_sem_pend(sema1);
        //Begin Critical Section

	    if(v==NRK_ERROR) nrk_kprintf( PSTR("T3 error pend\r\n"));
       	// wait some time inside semaphore to show the effect
    	spin_wait_s(5);	

        // End of Critical Section
    	v = nrk_sem_post(sema1);       

        // Give the scheduler a chance to breathe before this check
    	if(v==NRK_ERROR) nrk_kprintf( PSTR("T3 error post\r\n"));
        nrk_wait_until_next_period();
   }
}

void nrk_create_taskset()
{
  TaskOne.task = Task1;
  TaskOne.Ptos = (void *) &Stack1[NRK_APP_STACKSIZE];
  TaskOne.Pbos = (void *) &Stack1[0];
  TaskOne.prio = 1;
  TaskOne.FirstActivation = TRUE;
  TaskOne.Type = BASIC_TASK;
  TaskOne.SchType = PREEMPTIVE;
  TaskOne.period.secs = 15;
  TaskOne.period.nano_secs = 0;
  TaskOne.cpu_reserve.secs = 10;
  TaskOne.cpu_reserve.nano_secs = 0;
  TaskOne.offset.secs = 23;
  TaskOne.offset.nano_secs= 0;
  TaskOne.sems[0] = sema1;
//  TaskOne.sems[1] = sema2;
  TaskOne.num_sems = 1;
  nrk_activate_task (&TaskOne);

  TaskTwo.task = Task2;
  TaskTwo.Ptos = (void *) &Stack2[NRK_APP_STACKSIZE];
  TaskTwo.Pbos = (void *) &Stack2[0];
  TaskTwo.prio = 2;
  TaskTwo.FirstActivation = TRUE;
  TaskTwo.Type = BASIC_TASK;
  TaskTwo.SchType = PREEMPTIVE;
  TaskTwo.period.secs = 20;
  TaskTwo.period.nano_secs = 0;
  TaskTwo.cpu_reserve.secs = 10;
  TaskTwo.cpu_reserve.nano_secs = 0;
  TaskTwo.offset.secs = 0;
  TaskTwo.offset.nano_secs= 0;
  TaskTwo.sems[0] = sema2;
//  TaskTwo.sems[1] = sema1;
  TaskTwo.num_sems = 1;
  nrk_activate_task (&TaskTwo);

  TaskThree.task = Task3;
  TaskThree.Ptos = (void *) &Stack3[NRK_APP_STACKSIZE];
  TaskThree.Pbos = (void *) &Stack3[0];
  TaskThree.prio = 3;
  TaskThree.FirstActivation = TRUE;
  TaskThree.Type = BASIC_TASK;
  TaskThree.SchType = PREEMPTIVE;
  TaskThree.period.secs = 25;
  TaskThree.period.nano_secs = 0;
  TaskThree.cpu_reserve.secs = 10;
  TaskThree.cpu_reserve.nano_secs = 0;
  TaskThree.offset.secs = 27;
  TaskThree.offset.nano_secs= 0;
  TaskThree.sems[0] = sema1;
  TaskThree.num_sems = 1;
  nrk_activate_task (&TaskThree);
}



